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and  the  filter  gains  calculated  on-line.  Tests  were  con¬ 
ducted  using  simulated  torpedo  trajectories  that  traversed 
both  single  and  multiple  hydrophone  arrays.  It  was  found 
that  filter  performance  was  dependent  on  system  noise  and 
the  distance  the  torpedo  was  from  the  hydrophone  array. 
Position  and  depth  errors  ranged  between  0  and  10  feet. 


ABSTRACT 

A  sequential  Extended  Kalman  filter  routine  was  developed 
to  provide  real  time  estimates  of  torpedo  position  and  depth 
on  the  three  dimensional  underwater  tracking  range  at  the 
Naval  Torpedo  Station,  Keyport,  Washington.  Filter  measure¬ 
ments  consisted  of  acoustic  pulse  transit  times  from  the  tor¬ 
pedo  to  a  receiving  hydrophone  array.  These  measurements, 
which  are  nonlinear  functions  of  the  position  and  depth  coordi 
nates,  were  linearized  and  the  filter  gains  calculated  on-line 
Tests  were  conducted  using  simulated  torpedo  trajectories 
that  traversed  both  single  and  multiple  hydrophone  arrays. 

It  was  found  that  filter  performance  was  dependent  on  system 
noise  and  the  distance  the  torpedo  was  from  the  hydrophone 
array.  Position  and  depth  errors  ranged  between  0  and  10 
feet. 
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I .  INTRODUCTION 


The  Naval  Torpedo  Station  (NAVTORPSTA)  Keyport,  Washington 
currently  operates  two  three-dimensional  (3-D)  underwater 
tracking  ranges  with  acoustical  capability.  The  underwater 
tracking  range  utilizes  a  sonar  transmitter  installed  in  the 
torpedo  to  be  tracked.  The  transmitter  is  synchronized  with 
a  master  clock.  Timed  acoustic  pulses  are  received  by  bottom 
mounted  hydrophone  arrays  and  then  relayed  via  cable  to  a  com¬ 
puter  at  the  observation  site.  The  computer  calculates  the 
positional  coordinates  of  the  torpedo  and  plots  its  trajectory 
through  the  water. 

The  measured  data,  which  consists  of  the  elapsed  time 
from  transmission  of  a  pulse  until  its  receipt  at  the  hydro¬ 
phone  array,  is  corrupted  with  noise  due  to  the  combined  ef¬ 
fects  of  environmental  factors  and  measurement  instruments. 

In  order  to  achieve  a  more  accurate  plot  of  the  torpedo 
path,  editing  and  smoothing  techniques  have  been  applied  to 
the  data  during  post  run  analyses. 

To  improve  the  real-time  capability  of  the  3-D  tracking 
ranges,  Kalman  filtering  theory  has  been  applied  to  the  com¬ 
puter  generated  X,  Y,  and  Z  positional  coordinates  but  is 
not  operational  [1]. 

In  the  near  future  an  updated  computer  system  composed 
of  three  MODCOMPIV  computers  will  be  in  operation.  The 
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majority  of  the  software  development  requires  interfacing 
current  tracking  programs  and  other  related  programs  to  the 
new  computer  system. 

An  opportunity  exists  for  expanding  the  capability  of 
the  tracking  system  by  applying  a  Kalman  filter  routine  which 
processes  measurements  of  the  transit  times  of  the  acoustic 
pulses  and  provides  an  estimate  of  the  position  of  the 
tracked  object  in  real  time.  Prior  research  in  this  area 
[2] ,  revealed  that  a  Kalman  filter  utilizing  a  sequential 
estimation  approach  was  desirable. 

The  intention  is  to  develop  and  test  a  sequential  Kalman 
filter  tracking  algorithm  that  can  be  interfaced  with  the 
current  underwater  range  system. 
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II.  DESCRIPTION  OF  RANGE  TRACKING  GEOMETRY 

The  hydrophone  array,  consisting  of  four  independent  ele¬ 
ments,  defines  an  orthogonal  coordinate  system  in  which  transit 
time  measurements  are  made.  As  shown  in  Figure  1,  four  hydro¬ 
phones  X,  Y,  Z,  and  C  are  on  four  adjacent  vertices  separated 
by  a  distance  d,  along  the  edge  of  the  cube.  The  origin  of 
the  array  coordinates  is  at  the  center  of  the  cube  with  the 
orthogonal  coordinates  parallel  to  its  edge.  Positional 
information  is  computed  from  the  transit  times  of  a  periodic 
synchronous  acoustic  signal  travelling  from  the  torpedo  to 
the  four  hydrophones  on  the  array.  The  range  measures  the 
tracked  torpedo's  position  every  1.31  seconds  to  an  accuracy 
that  is  typically  within  3  to  30  feet.  A  more  detailed  de¬ 
scription  of  the  range  tracking  capability  is  described  in 
[3]  . 
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Figure  1.  Geometry  of  a  Tracking  Array 


III.  THEORY — EXTENDED  KALMAN  FILTER 

The  equations  describing  the  nonlinear  relationship 

between  transit  times  T  ,  T  ,  Ty,  Tz  and  the  position  of 

* 

the  tracked  torpedo  are  given  by  [3] 

Tc  -  vfL  I(x  +  §>2  +  <y  +  !>2  +  <z  + 1)2>%  •  <3-n 

Tx  ■  vel  [lx  -  !>2  +  ‘y  +  !>2  +  <z  +  !)21*  •  (3-2> 

ty  '  vIi  1<x  +  f>2  +  -  !>2  +  ,z  +  i)2)%  -  (3-3) 

Tz  =  vSL  [(x  +  I)2  +  +  f)2  +  (z  -  ^)2]k.  (3.4) 

where  d  =  30  feet  and  VEL  =  4860,  which  is  approximately  the 
average  velocity  of  propagation  of  sound  in  water  at  Dabob 

•k 

Bay.  Since  the  transit  times  were  readily  available  and  are 
nonlinear  functions  of  position,  these  equations  can  be  linear¬ 
ized  and  Kalman  Filter  theory  applied  using  the  Extended  Kalman 
Filter  [2] .  This  procedure  producses  a  real  time  measurement, 
with  filtering  on  the  corrupted  transit  times  T^,  T^,  T  ,  and 
T„  without  the  necessity  of  converting  these  times  to  positions. 

*Sound  velocity  in  water  varies  as  a  function  of  temperature, 
salinity,  and  depth. 
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For  the  three-dimensional  location  problem  three  position 
states  (x,  y,  z)  and  two  velocity  states  (v  ,  v  )  specify 

x  y  -  * 

target  motion.  The  discrete  linear  and  nonlinear  observation 
equations  are  given  by 

x(k  +  1)  =  i  *  x(k)  +  r  •  w(k)  (3.5) 


and 


z(k)  =  h(x(k),k)  +  v(k) 


(3.6) 


In  these  equations  1  and  3  are  constant  matrices  and  h  is  a 
nonlinear  function  of  the  state  variable  x.  w(k)  is  plant 
excitation  noise  and  v(k)  is  measurement  noise.  The  plant 
noise  and  measurement  noise  are  assumed  uncorrelated  (white) 
with  zero  mean. 

That  is, 

E[w(k)  •  wT(j)  ]  =  Q*  (k)>5kj 

and 

E[v(k)  *  vT( j) ]  =  R(k) 5kj 

with 


k  =  j 


-  0,  k  fi  j 


In  order  to  apply  the  linear  filter  equation  (3.6)  is 
expanded  in  a  Taylor  series  about  the  best  estimate  of  the 
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the  state  at  that  time  and  only  the  first-order  terms  are 
kept. 

Equation  (3.6)  gives 


z (k)  =  H(k)  •  x (k)  +  v (k) 


(3.7) 


where 


x' (k)  =  x (k/k-1) 
x(k/k-l)  is  the  predicted  value  of  the  state  before  the 
kth  measurement. 

A  state  error  vector  is  defined  by 


H(k)  =  § 


(3.7a) 


x 


x  (k) 


-  x(k)  , 


and  a  predicted  state  error  vector  is  defined  by 
x(k/k-l)  =  x(k/k-l)  -  x(k)  . 


The  covariance  of  state  error  matrix  is  defined  by 
P (k)  =  E [x (k)  •  xT(k) ]  , 

and  the  predicted  covariance  of  state  error  matrix  is  given 
by 

P (k/k-1)  =  E[x(k/k-l)  •  xT (k/k-1)]  . 

The  state  excitation  matrix  is  given  by 
Q(k)  =  T(k)E[w(k)  •  wT(k)  ]  •  rT(k) 
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and  the  measurement  noise  covariance  matrix  is 


R(k)  =  E[v(k)  -  vT(k)  ] 


The  Kalman  filter  equations  are  given  by  [4,5]: 

P  (k+l/k)  =  4>P  (k/k)  4>T  +  Q  (k)  (3.8a) 
G (k)  =  P (k/k-1) HT (k)  [H(k)  .  P (k/k-1) HT (k)  +  R(k)]"1  (3.8b) 
P (k)  =  [I  -  G/ (k) H (k) ] P (k/k-1)  (3.8c) 
x (k+l/k  =  $x (k/k)  ( 3 . 8d) 
z (k/k-1)  =  h(x(k/k-l)  ,k)  )  (3.8e) 
x (k)  =  x(k/k-l)  +  G (k)  [ z (k)  -  z (k/k-1)]  (3.8f) 


The  Q  matrix  serves  not  only  to  allow  for  maneuvering  but 
also  to  account  for  any  model  inaccuracies,  that  is,  any  dis¬ 
crepancies  between  the  true  action  of  the  torpedo  and  its 
characterization  by  Equation  (3.5).  The  Q  also  serves  to 
prevent  che  gain  matrix  G(k)  from  approaching  zero  by  always 
insuring  uncertainty  in  the  predicted  covariance  of  error 
matrix  P (k+l/k). 
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IV.  PROBLEM  DEFINITION — TORPEDO  TRACKING  WITH  THE  EXTENDED 

KALMAN  FILTER 


A.  FILTER  EQUATIONS 

Figure  2  illustrates  the  geometry  of  the  states  used  for 
the  filter.  For  a  constant  course,  constant-speed  torpedo, 
the  plant  state  equations  can  be  described  as  two  second 
order  systems,  one  for  X  and  one  for  Y.  The  depth  (Z)  of  the 
torpedo  is  maintained  constant  and  any  velocity  in  the  Z  di¬ 
rection  is  assumed  to  be  random,  uncorrelated  with  zero  mean. 
Z  is  therefore  described  as  a  first  order  system. 


The  plant  state  equations  are 


r 

r  -i 

x (k+l) 

x(k)+Tv  (k)+g  (y  •  , y.  ,k) 

X  J- 

vx(k+l) 

vx(k)+g2(Y^  ,Yv  'k) 

-t  t 

y (k+l) 

y(k)+Tv  (k)+g  (y.  ,y.  ,k) 
y  J  vt 

v  (k+l) 

i 

vv(k)+94^i  fY  •  k> 

y  4  "t  vt' 

z (k+l) 

z(k)+g5(yz) 

where  x(k) ,  and  y(k)  are  the  position  coordinates  at  time 

'  Vk)  and  Vy^  are  the  x  and  Y  components  of  velocity. 
T  is  the  time  between  observations,  i.e.  t(k+l)-t(k)  and  z(k) 
is  the  depth  of  the  torpedo. 


(0,0,0) 


acoustic  center 

HYDROPHONE  ARRAY 


STATES 


x 

y 

v 


Figure  2.  Filter  Geometry 


The  excitation  terms  g^  through  g^  are  included  to  account 
for  the  random  changes  in  speed,  heading,  and  depth  which  can 


occur  in  a  maneuvering  torpedo.  The  quantities  Y 


V 


o  ,  and 
t 


Yz  are  the  random  changes  of  the  torpedo  which  are  assumed 
to  be  independent,  zero  mean,  rates  of  change.  They  have 
variances  defined  by 


1 


=  E  lve  21 

2  o 

'  i  =  Ehz2] 


The  values  of  the  standard  deviations  are  taken  from  typical 
maneuvering  parameters  for  the  torpedo; 


=  22°/sec 

=  36  ft/sec^ 
=  1  ft/sec 


The  effect  of  this  excitation  is  to  increase  the  predicted 
value  of  the  covariance  of  the  state  error  matrix. 

Writing  the  equations  in  state  form  resuts  in 


x (k+1)  =  ?x  (k)  +  Tw(k) 


(4.2) 
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where 


and 


The  vector  w(k)  represents  the  effect  on  the  states  of  the 
random  excitations.  Reference  5  has  shown  that  the  vector 
may  be  calculated  from  the  equations  relating  torpedo  X  and 
Y  velocity  to  the  torpedo  heading  and  velocity.  The  X 
velocity  is 


X  =  V  =  V  cos  t? 

«  L  U 


which  when  differentiated  gives 


x  =  -vjsine  +  vtcoset 


(4.4) 
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where 


9 


t 


t 


v 


t 


sin  9.  =  — ^ 


r\ 

COS  0^  =  - 


and  upon  substitution  of  the  above  relationship  in  equation 
(4.4)  , 


x  = 


(4.5) 


Similarly 


y 


vtsin 


et 


(4.6) 


y  =  vt"tcos"t  +  ^tsin6t 

and  after  substitution 


y 


vxYei 


(4.8) 


The  depth  term  is  just 


z 


(4.9) 
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XI 


Thus 


v(k) 


T 


v.  ) 

y 


(Y, 


+ 


V 

n*> 


where  from  the  assumption  on  the  y  's 


(4.10) 


E [w (k)  ]  =  0 


(4.11) 


The  excitation  covariance  matrix  is  thus  found  from 


Q  =  rE  [w(k)w(k)T]  PT 


Let 


j.. 

x 


2 


2 


Jy 


2 


t 


a. .. 

xy 


V  V 


x  y 


1  ] 

'  t 


(4.12) 


(4.13a) 


(4.13b) 


(4.13c) 


where  the  states  are  evaluated  at  the  current  state  estimates 
(k) .  Substituting  these  expressions  in  the  Q  matrix  results 
in 
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Q  = 


2^2  3  2  22  3 

(T  AP--  T  / 0  a ..  (T  / _  )a.. ..  T  /„  o~a..  0 

'  '  2 '  x  7  2  x  '2  xy  2xy 


2  2  3  2 
To..  T  /2  a--  T^a-  •• 

x  '  *  xy  xy 


(T2/,)23..2  T3/-.a..2 


I’2,..2 


Symmetric 


T2g-2 


(4.14) 


The  excitation  matrix  serves  not  only  to  take  into  ac¬ 
count  the  possibility  of  maneuvering,  but  of  model  inaccuracies 
as  well  [5] .  Q  is  also  used  to  prevent  the  gains  of  the  filter 
from  approaching  zero  as  more  and  more  data  is  processed,  by 
insuring  some  uncertainty  in  the  predicted  state  values. 

The  observation  equations  are  nonlinear  in  the  states 
and  are  given  by 


r  i 

—  — 

Vk) 

^|i-[(x(k)+d/2)2+(y(k)+d/2)2+(z(k)-Kl/2)2]^  +  v(k) 

Vk) 

^-((x(k)-d/2)2+(y(k)+d/2)2+(z(k)+d/2)2]^  +  v(k) 

Ty(k) 

(x(k)+d/2) 2+(y(k)-d/2) 2+(z(k)+d/2) 2]^  +  v(k) 

i 

M 

H 

_ 1 

^-[(x(k)+d/2)2+(y(k)+d/2)2+(z(k)-d/2)2]^  +  v(k) 

(4.15) 
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The  measurement  noises,  v(k)'s,  are  assumed  to  be  zero-mean 
and  independent  with  a  covariance  matrix 


R(k) 


0 

0 


0 


0 

0 


0 


0 


0 


(4.16) 


The  magnitude  of  the  transit  times  noise  is  a  function  of 
the  signal-to-noise  ratio  at  the  hydrophone  array,  which  is 
influenced  by  several  environmental  factors. 

Discussions  with  NAVTORPSTA  personnel  indicate  a  typical 
accuracy  of  +  10  microseconds  and  this  value  is  used  as  the 
standard  deviation  for  the  simulation  runs. 

Equation  (3.7a)  can  be  used  to  give  the  linearized  observa¬ 
tion  matrix.  The  result  is 


~?Tc 

3Tc(k) 

3Tc(k) 

3Tc(k) 

3Tc(k) 

oV 

X 

3y 

3v 

y 

3z 

3Tx(k) 

3Tx(k) 

3Tx(k) 

3Tx(k) 

3Tx(k) 

6x 

ivx 

ay 

3v 

y 

3z 

3Ty(k) 

3Ty(k) 

3Ty  (k) 

3Ty(k) 

3Ty (k) 

3x 

3v 

X 

3y 

3v 

y 

~Tz 

3Tz(k) 

3Tz(k) 

3Tz(k) 

3Tz(k) 

3Tz(k) 

oX 

3vx 

sy 

5v 

V 

3z 
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When  the  derivatives  are  taken  and  evaluated  at  the  pre¬ 


dicted  state  values  x(k/k-l)  =  x'(k)  the  result  is 


where 

DENI  =  I(x' (k)+d/2) 2+(y’ (k) +d/2) 2+ ( z ' (k)+d/2)2]^ 

DEN 2  =  [(x-  (k)-d/2)2+(y>  (k)+d/2)2+(z*  (k)+d/2)2]^ 

DEN 3  =  [(x' (k)+d/2) 2+{y' (k) -d/2) 2+ ( z ' (k)+d/2)2]^ 

DEN 4  =  [(x' (k)+d/2)2  +  {y’  (k)+d/2) 2+(z'  (k)-d/2) 2]^  . 

The  4>  matrix,  Q  matrix,  R  matrix,  and  H  matrix  are  then 
used  in  the  Kalman  filter  equations  (3.8). 

B.  THE  SEQUENTIAL  EXTENDED  KALMAN  FILTER 

The  Kalman  filter  equations  described  in  the  previous 

section  represent  a  system  that  is  characterized  by  a  one- 

step  estimation  and  prediction  for  each  set  of  four  transit 

time  observables  (T  ,  Tv,  T„,  and  T_) .  This  one  step  process 

C  X  Y  i* 

exhibits  the  following  salient  features: 


1.  The  four  transit  times  received  during  each  1.31 
second  measurement  interval  (time  slot)  are  processed  simul¬ 
taneously  by  the  filter  equations.  Updated  covariance  of 
error  and  state  estimates  are  therefore  obtained  only  once 
for  each  time  slot. 

2.  A  major  part  of  the  time  period  for  the  estimation 
calculation  is  spent  in  the  gain  equation  where  the  inversion 
of  a  4  x  4  matrix  is  required. 

3.  The  Extended  Kalman  filter  is  based  on  a  linearization 
about  the  predicted  value  of  a  period  t^  to  t^+1.  Unaccept¬ 
ably  large  or  erroneous  transit  time  measurements  will  cause 
the  filter  to  ignore  the  update  information  for  the  entire 
time  slot.  The  predicted  estimate  will  become  the  new  esti¬ 
mate  for  the  time  slot  and  the  interval  extended. 

If  one  assumes  that  each  of  the  four  hydrophones  in  the 
array  are  statistically  independent  of  one  another,  a  se¬ 
quential  approach  can  be  taken  to  process  each  of  the  four 
transit  times  separately.  Thus,  each  observable  will  be 
processed  in  sequence,  and  the  result  of  processing  one 
measurement  is  used  in  the  following  computation  to  process 
the  next  measurement  until  all  four  observables  have  been 
used . 

The  advantages  of  the  sequential  approach  over  the  tra¬ 
ditional  one-step  estimation  and  prediction  are; 

1.  Since  the  four  transit  times  are  independent  and 
processed  sequentially,  the  covariance  of  error  matrix  and 
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the  state  vector  are  updated  four  times  during  each  time 
slot.  Thus  more  accurate  estimates  of  the  filter  states 


are  achieved. 

2.  Modification  of  the  filter  equations  for  the  sequen¬ 
tial  approach  circumvented  the  matrix  inversion  in  the  gain 
equation.  Therefore  filter  cycle  time  is  reduced  substantially. 

3.  An  invalid  transit  time  measurement  will  result  in 
the  filter  ignoring  the  update  information  for  that  particular 
measurement  only.  For  example,  if  only  three  of  the  four 
transit  times  are  acceptable,  the  filter  would  be  sequentially 
updated  three  times  during  that  particular  time  slot.  Thus, 
the  filter  is  less  sensitive  to  erroneous  transit  time 
measurements . 

To  utilize  the  sequential  Extended  Kalman  filter,  equations 
(3.8)  must  be  modified.  Calculations  are  performed  on  each  of 
the  four  independent  transit  times  in  the  following  order:  Tc, 
Tx,  T  ,  and  for  each  1.31  second  time  slot.  The  estimate 
of  the  states  x(k/k),  based  on  one  transit  time  measurement 
are  used  as  the  prediction  x(k/k-l)  for  the  calculations  on 
the  next  measurement.  Thus  for  the  first  time  measurement  Tc 
only  the  first  row  of  the  linearizing  H  matrix  is  calculated. 

Next  the  first  gain  column  corresponding  to  the  first 
time  measurement  Tc  is  calculated  by 
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(4.19) 


where  i  =  1  to  4  corresponding  to  the  four  measured  transit 
times.  Thus,  the  first  row  of  the  H  matrix  is  used  to  calcu¬ 
late  the  first  column  of  the  gain  matrix  with  both  correspond¬ 
ing  to  the  first  measured  time  Tc . 

Next,  an  estimate  of  the  particular  observation  time  is 
calculated  using  equation  (3.8e)  evaluated  at  the  predicted 
state  x(k/k-l) . 

The  difference  between  observed  transit  time  and  the 
estimated  transit  times  forms  the  residual  which  is  used  in 
the  estimate  equation 

x.  =  x(k/k-l)  +  G.  , [Residual]  (4.20) 

1  ICO  -L 

This  equation  gives  an  estimate  of  the  states  based  on  one 
measurement. 

Next,  the  covariance  of  error  is  calculated  based  on  one 
measurement  by 
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(4.21) 


where 

I  =  identity  matrix 

p 

i-1  =  the  covariance  matrix  calculated  from  the  pre¬ 
vious  transit  time  measurement  or  if  i  =  1, 
the  prediction  P(k/k-l). 

After  the  first  iteration,  becomes  x(k/k-l)  and  P^  becomes 
P(k/k-l)  for  the  second  iteration  which  calculates  the  estimate 
of  the  states  based  on  the  second  measurement  T^. 
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After  four  iterations  (k  =  4) ,  x^  becomes  the  estimate  for 
the  time  slot  x(k/k)  and  P^  becomes  the  covariance  of  error 
P (k/k)  . 

The  predictions  for  the  next  time  slot  are  calculated 


using  equations  (3.8a)  and  (3.8d).  This  process  is  repeated 
for  each  time  slot. 


IV.  TESTING  AND  SIMULATION 


A.  DESCRIPTION 

The  sequential  Extended  Kalman  filter  routine  is  tested 
using  simulated  torpedo  tracks  generated  by  the  IBM-360  com¬ 
puter.  A  variety  of  track  scenarios  are  produced  to  test  the 
filter  performance  during  single  array  tracking. 

The  first  series  of  tests  used  constant  course  and  speed 
tracks  which  transitioned  multiple  array  quadrants  including: 

1.  crossing  north  of  the  array 

2.  crossing  above  the  array 

3.  crossing  diagonally  through  the  array 

The  next  series  of  tests  adds  a  number  of  typical  torpedo 
maneuvers  to  the  constant  course  and  speed  tracks.  These  ma¬ 
neuvers  consisted  of  10  and  20  °/sec  turns  with  G  forces 
ranging  from  1/4-G  to  1-G. 

For  the  above  tests  filter  initialization  errors  range 
from  0  to  40  feet/sec  for  X  and  Y  velocity,  and  0  to  60  feet 
for  X  and  Y  position.  The  Z  depth  is  maintained  constant  at 
300  feet  which  is  approximately  the  average  depth  of  water 
at  Dabob  bay.  Zero  mean  Gaussian  noise  is  added  to  corrupt 
the  observed  transit  times  for  all  runs . 

The  last  series  of  tests  demonstrates  the  ability  of 
the  filter  to  track  through  the  areas  of  multiple  arrays. 

Before  the  above  series  of  tests  can  be  conducted,  it 
is  necessary  to  add  operational  features  to  the  software  which 
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provide  the  following  capability: 

1.  detect  erroneous  transit  time  measurements 

2.  minimize  position  error  during  high  dynamic  maneuvers 

3.  track  through  multiple  arrays 

A  more  detailed  explanation  of  these  features  is  described 
below. 

B.  EDITING  ERRONEOUS  TIME  MEASUREMENTS 

The  operation  of  the  filter  may  be  adversely  affected  by 
erroneous  transit  time  measurements.  Large  errors  in  the 
transit  time  measurements  will  produce  errors  in  the  state 
vector  estimate  and  the  linearizing  H  matrix.  This  may  cause 
the  filter  to  become  unstable  and  exhibit  divergent  character¬ 
istics.  In  some  extreme  cases  catastrophic  failure  may  result. 
Measurement  errors  can  occur  because  of  many  factors  including 
an  error  in  the  transit  time  of  the  acoustic  pulse  primarily 
due  to  the  receipt  of  multi  path  signals  from  previous  time 
slots  that  have  reflected  off  the  surface,  bottom,  or  different 
density  layers  in  the  water. 

To  guard  against  catastrophic  failures  a  three-sigma  gate 
using  the  covariance  of  measurement  noise  (R)  and  the  covari¬ 
ance  of  estimation  error  (P(k/k))  is  implemented.  For  each 
calculation  of  a  state  estimate  (x(k/k) ) ,  the  largest  posi¬ 
tional  covariance  of  error  is  used,  either  x,  or  z,  and 
converted  to  time  in  seconds  using  the  average  velocity  of 
sound  in  water  for  Dabob  bay,  4860  ft/sec.  The  gate  then  is 
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written  for  each  time  measurement  i  =  1  to  4: 

^ _ \j  (Pj j)maximum+Rii 

Cate - - 

where  j  =  1,  3,  5.  The  gate  expands  or  decreases  depending 
on  the  confidence  level  of  the  position  estimate  and  the 
transit  time.  If  ZDIFF  which  is  the  difference  between  the 
actual  transit  time  received  and  the  predicted  transit  time 
to  a  particular  hydrophone  exceeds  the  gate,  the  measurement 
is  considered  unacceptable  and  the  filter  gain  is  set  to 
zero  causing  the  filter  to  ignore  the  data  and  take  the  pre¬ 
diction  of  the  states  as  the  estimate 

x(k/k)  =  x (k/k-1) 

An  invalid  time  measurement  zeros  only  the  gain  column  for 
that  particular  hydrophone  causing  only  that  hydrophone's 
data  to  be  ignored. 

C.  BOUNDING  RESIDUAL  BIAS  ERROR 

As  previously  mentioned,  the  torpedo  dynamics  are  modeled 
in  the  filter's  excitation  covariance  matrix  Q. 

During  preliminary  tests  it  becomes  evident  that  torpedo 
turn  rates  between  5  and  20°/sec  yield  position  errors  ranging 
from  approximately  10  to  40  ft.  Increasing  the  standard  devi¬ 
ation  parameters  (a^  a  c^)  for  the  adaptive  Q  matrix  helps 
to  a  certain  extent,  however  as  expected  a  point  is  reached 
where  the  noise  starts  to  become  the  dominant  error  source. 
There  are  two  major  reasons  why  the  filter  develops  errors 
during  turns; 


*  i 
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1.  The  filter’s  predicted  state  estimates  are  computed 


assuming  a  constant  velocity  over  a  1.31  second  sampling 
interval.  During  a  turn  the  velocity  vector  of  the  torpedo 
is  continuously  rotating,  therefore  the  filter's  predicted 
estimate  will  be  in  error.  A  large  magnitude  error  will  re¬ 
quire  more  than  one  time  slot  to  remove.  A  faster  sampling 
rate  would  reduce  the  error  and  may  alleviate  the  problem; 
however,  the  Dabob  tracking  range  system  has  been  designed 
for  a  1.31  second  sampling  rate  and  cannot  be  changed. 

2.  The  adaptive  Q  matrix  assumes  that  the  torpedo  ac¬ 
celerations  are  normally  distributed  and  uncorrelated.  Torpedo 
tracking  maneuvers  do  not  fit  these  assumptions. 

To  minimize  position  errors  during  maneuvers  a  procedure 
has  been  developed  in  order  that  repeated  estimates  of  the 
state  vector  are  obtained  during  a  single  sampling  period 
(1.31  second  time  slot)  until  the  transit  time  residual  error 
falls  below  a  preset  threshold. 

In  Section  IV  it  was  stated  that  during  each  time  slot 
the  gain,  covariance,  and  state  estimate  equations  (4.19- 
4.21)  were  iteratively  solved  for  each  of  the  four  sequential 
transit  times  Tc,  T^,  Ty,  and  Tz.  Subsequently  additional 
software  was  incorporated  to  calculate  the  estimated  transit 
time  to  each  of  the  four  hydrophones  based  on  the  last  up¬ 
dated  state  vector.  Four  time  residuals  are  then  computed 
by  subtracting  the  estimated  time  to  each  hydrophone  from  the 
observed  transit  time.  The  average  of  the  absolute  value  of 


the  time  residuals  is  then  formed,  if  the  average  residual 
error  exceeds  the  preset  threshold  in  the  software,  which  is 
indicative  of  large  error  in  the  state  vector,  Q  is  calculated 
and  added  to  the  last  updated  covariance  of  error  matrix  P. 

The  filter  then  reiterates  the  gain,  covariance,  and  state 
estimate  equations  for  the  same  time  slot.  If  the  new  state 
vector  generates  an  average  residual  error  still  exceeding  the 
threshold  value,  then  the  process  is  repeated.  This  procedure 
continues  until  the  average  residual  error  falls  below  the 
preset  threshold  at  which  time  an  acceptable  state  vector 
estimate  has  been  obtained  for  the  time  slot.  Once  this  oc¬ 
curs  the  prediction  equations  (3.8a)  and  (3.8d)  are  solved 
and  the  filter  proceeds  to  the  next  time  slot  and  commences 
processing  new  transit  time  measurements .  As  will  be  shown 
later  the  filter  did  successfully  bound  the  position  error 
during  turning  maneuvers.  For  these  tests  the  threshold  con¬ 
stant  is  set  at  10  microseconds,  which  is  the  value  of  RMS 
measurement  noise. 

C.  MULTIPLE  ARRAY  TRACKING 

Initial  tests  were  performed  on  tracks  in  the  area  of 
one  array.  In  order  to  more  closely  simulate  a  typical  run 
on  the  range,  a  scheme  was  designed  to  track  the  torpedo 
through  multiple  arrays  [2] . 

First,  a  coordinate  system  is  defined  as  shown  in  Figure 
3.  The  center  of  the  coordinate  system  is  geographically 
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near  the  entrance  to  Dabob  bay  in  the  simulation.  Array  num¬ 
ber  6  is  the  closest  array  to  the  coordinate  center.  In  the 

simulation  array  1  is  at  36,000  feet  from  coordinate  center 
and  array  6  is  6,000  feet.  The  C  hydrophone  is  assumed  to  be 

the  axis  location  of  each  array.  Then  each  X  position  for  the 

X  hydrophone  in  each  array  is  X^  +  30,  each  Y  position  for  the 
Y  hydrophone  is  Y +  30  and  each  Z  position  for  the  Z  hydro¬ 
phone  is  Zc  +  30.  These  72  positions,  an  XYZ  position  for 
each  of  4  hydrophones  in  6  arrays,  are  placed  into  a  6  x  12 
matrix  HYDRO  and  referenced  throughout  the  routine.  The 
torpedo  position  is  referenced  to  a  central  level  rectangular 


coordinate 

system.  The  nonlinear  observation  equations  become 

Tc(k) 

^[(x*  (k)-X.c)2+(y’  (k)-Y.c):+U’  (k)-Z.c)2]% 

z  (k)  = 

Vk) 

t4l((*  '(k)-X.x)2+(y’  (k)-Y.x)2+(Z’  (k)-Z.x)2)% 

Vk) 

^[(x'  (k)-X.Y)2+(y'  (k)-YiY)2+(2’(k)-Z.Y)2]% 

Vk)_ 

VELIU'(k)-Xi2)2+(^'(k)-Yi2)'+(z'(k,-ZiZ>‘l% 

___  — 

where 

Tc(k) 

,  Tv(k) ,  T  (k),  and  T  (k)  are  the  transit  times  to 

X  Y  u 

each  of  the  four  hydrophones  and  X' (k) ,  Y' (k) ,  and  Z' (k)  are 
the  filter  estimates  x'(k/k)  evaluated  at  the  predicted 
positions  x(k/k-l) .  The  subscripted  variables  X,  Y,  and  Z 
are  the  coordinates  of  a  particular  array  being  used. 
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The  decision  parameter  used  to  determine  the  switching 
from  array  to  array  is  a  straight  handoff.  If  the  predicted 
x  position  is  greater  than  3,000  feet  from  the  array  in  use, 
then  an  index  (18)  is  incremented  and  the  next  row  of  HYDRO 
is  implemented.  This  placed  into  the  routine  the  X,  Y,  and  Z 
positions  of  the  hydrophones  in  the  next  array.  The  handoff 
can  easily  be  utilized  in  real  range  operations,  as  the 
transit  times  from  adjacent  arrays  are  present  at  the  compu¬ 
ter  for  a  particular  time  slot.  For  simulation,  it  is  as¬ 
sumed  that  in  all  the  arrays  each  axis  pointed  in  the  same 
direction.  In  actual  range  operations  each  array  is  tilted 
about  both  the  X  and  Y  axis.  Since  the  true  transit  times 
are  derived  in  a  tilted  coordinate  frame,  the  filter's  esti¬ 
mate  of  transit  time  must  also  be  calculated  in  a  tilted 
coordinate  frame.  The  tilt  angle  measurements  along  with 
the  level  rectangular  coordinates  of  the  array  with  respect 
to  the  central  rectangular  coordinate  system  can  be  input 
into  the  matrix  HYDRO  to  rotate  the  coordinates  of  each 
hydrophone  in  the  array. 


VI.  TEST  RESULTS  AND  DISCUSSION 


A.  SERIES  ONE — STRAIGHT  RUNS 

This  series  of  tests  includes  constant  speed  and  heading 
tracks.  Initial  position  and  velocity  errors  are  set  to 
zero.  Measurement  noise  is  added  to  all  runs.  Torpedo  ve¬ 
locity  ranged  from  20  to  55  knots.  Figure  4  shows  the  true 
trajectory  of  the  torpedo  in  the  horizontal  X-Y  plane.  The 
center  of  the  hydrophone  array  is  located  at  positional  co¬ 
ordinates  (0,0).  Figure  5  describes  the  filter's  RMS  estimate 
of  position  error  as  a  function  of  sampling  time.  The  RMS 
estimates  are  obtained  by  taking  the  square-root  of  the  ap¬ 
propriate  diagonal  terms  of  the  covariance  matrix.  It  should 
be  noted  that  the  propagation  of  RMS  estimates  are  a  function 
of  the  position  of  the  torpedo  with  respect  to  the  hydrophone 
array.  As  shows  in  Figure  4,  if  Y  =  0,  the  torpedo  path  is 
inbound  toward  the  array  along  the  "X"  axis.  Upon  examining 
Figure  5,  it  is  evident  that  the  RMS  estimate  of  X  position 
error  or  the  uncertainty  in  the  filter's  X  position  estimate 
is  small  and  relatively  constant  throughout  the  entire  tra¬ 
jectory.  The  uncertainty  in  the  filter's  Y  position  estimate 
decreases  as  the  torpedo  approaches  the  array,  becomes  a  mini¬ 
mum  at  the  closest  point  of  approach  (CPA) ,  and  increases  as 
the  torpedo  moves  outbound  away  from  the  array.  This  behavior 
in  the  filter's  RMS  estimates  can  be  explained  by  recognizing 


that  the  cluster  of  four  hydrophones  in  the  array  actually 
describe  a  position  fixing  system.  The  torpedo  position  is 
located  by  the  intersection  of  circular  lines  of  position 
(range  arcs)  originating  from  each  hydrophone  on  the  array. 

In  general,  the  greater  distance  the  torpedo  is  located  from 
the  array,  the  shallower  the  range  arc  intersections  become 
and  the  more  uncertainty  exists  in  the  actual  torpedo  posi¬ 
tion.  For  the  case  where  the  torpedo  is  moving  along  the  X 
axis,  the  uncertainty  in  position  exists  only  in  the  Y 
direction.  If  the  torpedo's  X  distance  from  the  array 
becomes  infinite,  the  range  arcs  from  each  hydrophone  appear 
to  originate  from  a  single  point  source.  The  position  fixing 
system  then  reduces  to  a  single  dimension  ranging  system  along 
the  X  axis  with  the  uncertainty  in  the  Y  position  estimate 
becoming  infinite.  If  the  torpedo  approaches  the  array  along 
the  Y  axis  (X  =  0) ,  the  filter's  RMS  estimate  of  Y  position 
error  will  be  relatively  small,  and  as  the  torpedo's  Y  distance 
from  the  array  becomes  infinite,  the  filter's  uncertainty  in 
the  X  position  estimate  becomes  infinite. 

In  order  to  keep  RMS  position  error  estimates  within 
allowable  tolerance  the  Dabob  range  tracking  system  requires 
the  torpedo  remain  within  a  3,000  foot  radius  of  the  center  of 
the  hydrophone  array. 

Figures  6  and  7  depict  the  error  in  both  position  and 
depth  for  the  straight  run  trajectory  along  the  X  axis. 

The  position  errors  are  computed  by  subtracting  the  filter 
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position  estimate,  x(k/k),  from  the  computer  generated  true 
position  for  each  time  slot.  From  Figure  6,  it  is  observed 
that  the  Y  position  error  magnitude,  which  is  caused  by 
system  noise, decreases  on  the  inbound  leg  and  increases  on 
the  outbound  leg.  Since  the  torpedo  path  originated  on  the 
outer  tracking  perimeter  (X  =  3,000  feet),  the  noise  in  the 

Y  channel  will  be  amplified  due  to  the  large  uncertainty  in 
the  Y  position  estimate.  The  magnitude  of  noise  error  in  the 

Y  direction  also  decreases  as  the  torpedo  approaches  the  ar¬ 
ray  and  increases  as  the  torpedo  moves  outbound.  The  steady 
state  X  and  Y  position  errors  ranged  between  -11  and  +4  feet 
throughout  the  trajectory. 

Figure  8  describes  a  straight  inbound  torpedo  path  that 
passes  north  of  the  array  at  a  constant  Y  distance  of  2121 
feet.  Torpedo  velocity  is  maintained  at  20  knots.  A  compari¬ 
son  of  the  RMS  x  estimates  of  this  track  with  the  preceding 
one,  Figures  5  and  9,  reveal  that  both  estimates  are  relatively 
constant,  however  the  X  estimate  in  Figure  9  is  significantly 
larger.  Since  the  torpedo  path  is  parallel  to  the  X  axis,  a 
larger  uncertainty  in  the  X  position  estimate  will  result. 

A  comparison  of  the  RMS  Y  position  error  estimates  reveals 
identical  propagation  characteristics.  This  is  due  to  the 
symmetry  in  the  east-west  direction  of  the  two  trajectories. 

Figures  10  and  11  show  that  the  steady  state  position 
and  depth  magnitude  errors  are  less  than  10  feet.  The  noise 


induced  error  in  the  X  position  is  due  to  the  uncertainty  in 
the  filter  X  position  estimate. 

A  torpedo  path  which  is  inbound  along  a  45°  diagonal  to 
the  array  is  depicted  in  Figure  12.  For  this  run  torpedo 
velocity  is  constant  at  28  knots.  Figure  13  shows  that  the 
filter's  X  and  Y  RMS  estimates  of  position  error  are  approxi¬ 
mately  equal.  Likewise  Figure  14  shows  that  X  and  Y  position 

error  propagation  is  also  similar.  These  results  are  consis¬ 
tent  since  the  X  and  Y  position  magnitudes  of  the  torpedo 
trajectory  are  identical.  At  the  point  of  closest  approach 

(X  =  0,  Y  =  0)  the  uncertainty  in  both  the  X  and  Y  positions 

becomes  a  minimum  and  increases  as  the  torpedo  moves  past 
the  array.  Steady  state  position  and  depth  errors  range 
between  -4  and  +6  feet. 

B.  SERIES  TWO— MANEUVERING  RUNS 

The  results  of  the  straight  run  analyses  discussed  in  the 
previous  subsection  show  that  the  propagation  of  the  filter's 
estimate  of  RMS  position  error  is  dependent  on  the  path  of 
the  torpedo  with  respect  to  the  hydrophone  array.  Thus  the 
erros  in  X  and  Y  position,  and  Z  depth  are  also  a  function  of 
the  torpedo  trajectory.  In  this  series  of  tests  maneuvers 
are  added  to  the  three  straight  path  trajectories.  These 
maneuvers  consist  of  360°  turns  at  turn  rates  of  10  and  20°/sec 
G  forces  range  between  1/4-G  and  1-G.  G  forces  are  changed 
using  the  same  turn  rate  by  varying  velocity.  A  typical  test 


run  on  the  tracking  range  at  Dabob  would  include  these  type 
maneuvers . 

Figure  16  describes  an  inbound  torpedo  path  which  main¬ 
tains  a  Y  distance  from  the  array  between  -200  and  +600  feet. 
Initial  position  and  velocity  are  30  and  20  feet  in  X  and  Y 
position  and  30  and  -20  feet/second  in  X  and  Y  velocity. 

Torpedo  velocity  is  maintained  constant  at  20  knots  through¬ 
out  the  trajectory.  Four  360°  1/4-G  turns  are  executed  during 
the  run.  Figures  17  and  18  describe  the  position  and  depth 
errors.  Within  approximately  three  time  slots  the  filter 
achieved  steady  state  performance.  Upon  observing  the  error 
propagation  it  is  apparent  that  the  filter  is  insensitive  to 
turn  rate  maneuvers.  Negligible  degradation  in  position  and 
depth  is  observed.  Thus  the  error  bound  routine  discussed  in 
the  previous  section  successfully  bounds  position  and  depth 
errors  during  turns.  As  expected  the  position  errors  decrease 
as  the  torpedo  approaches  the  array  and  increase  as  the  torpedo 
proceeds  outbound  from  the  array.  Since  position  and  depth 
errors  exhibit  approximately  equal  oscillations  about  zero 
indicates  that  the  system  noise  is  the  dominant  error  source 
driving  the  filter.  Bias  errors  on  the  time  residuals  due 
to  maneuvers  are  negligible.  Steady  state  errors  in  position 
and  depth  ranged  between  -1  and  +5  feet  throughout  the  run. 

The  next  test  describes  an  inbound  torpedo  trajectory, 
Figure  19,  which  passes  north  of  the  hydrophone  array.  The 
torpedo  maintained  a  Y  distance  from  the  array  between  1600 
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and  2400  feet.  Four  10°/sec  1/4-G  turns  are  executed  during 
this  test.  Filter  initialization  errors  are  20  and  11  feet 
in  X  and  Y  position  and  20  feet/sec  in  both  X  and  Y  velocity. 
The  error  in  position  and  depth  is  described  in  Figures  20 
and  21.  The  X  position  error  demonstrates  larger  deviations 
than  the  Y  position  error.  As  noted  during  the  straight  runs 
this  is  due  to  the  difference  in  the  filter's  X  and  Y  RMS 
position  error  estimates  for  this  type  trajectory.  The  oscil 
latory  characteristics  of  the  position  error  indicate  again 
that  noise  is  the  dominant  error  source  and  the  filter  is 
tracking  the  torpedo  through  the  maneuvers.  Position  errors 
range  between  -8  and  +8  feet  with  one  X  channel  error  reach¬ 
ing  a  maximum  of  -15  feet,  which  occurred  at  time  slot  140. 
The  exact  cause  of  the  -15  foot  error  is  unknown  but  may 
have  been  caused  by  an  anomaly  in  the  IBM-360  subroutine 
SNORM  which  generates  the  zero  mean  Gaussian  noise  for  the 
filter.  Errors  in  torpedo  depth,  Figure  21,  are  consistent 
and  range  between  -2  and  +5  feet. 

For  the  next  test  the  torpedo  exhibits  an  inbound  tra¬ 
jectory  along  a  45°  diagonal  with  respect  to  the  array. 

Figure  22  shows  that  four  10°/sec  1/4-G  turns  are  executed 
on  this  run:  2  on  the  inbound  leg  and  2  on  the  outbound  leg. 
Filter  initialization  errors  are  60  feet  in  both  X  and  Y 
position  and  30  feet  in  X  and  Y  velocity.  Steady  state  con¬ 
ditions  are  achieved  in  approximately  four  time  slots.  As 
expected  for  this  type  trajectory,  the  X  and  Y  position 
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errors  portray  similar  propagation  characteristics,  Figure  23. 
This  similarity  can  be  explained  by  the  equivalent  X  andY  RMS 
error  estimates  generated  by  the  filter  for  a  45°  diagonally 
inbound  trajectory.  Position  and  depth  errors,  Figure  24,  rnged 
ranged  between  -7  and  +10  feet. 

The  preceeding  tests  discussed  filter  performance  for 
straight  runs  with  10°/sec  1/4-G  turns.  Using  the  same  basic 
torpedo  trajectories  as  above,  similar  tests  are  performed  with 
10°/sec  turn  rates  at  1/2-G.  Figures  25  through  33  describe 
the  torpedo  paths  and  the  corresponding  position  and  depth 
error  propagation  plots  for  each  run.  Filter  initialization 
errors  ranged  between  10  and  40  feet  in  position  and  10  and 
40  feet  in  velocity.  Torpedo  velocity  is  maintained  constant 
at  55  knots.  An  analysis  of  the  position  and  depth  plots 
reveals  that  filter  performance  is  similar  to  that  described 
in  the  previous  rests.  In  Figures  26  and  32  the  larger  errors 
incurred  after  time  slot  135  are  due  to  the  torpedo  trajectory 
exceeding  the  radial  tracking  range  limit  of  3,000  feet.  This 
is  more  clearly  described  in  the  torpedo  trajectory  curves, 
Figures  25  and  31.  The  additional  1/4-G  acceleration  during 
the  turns  did  not  produce  visible  degradation  in  the  filter's 
position  and  depth  estimates. 

The  next  set  of  maneuvering  tests  increased  the  turn  rate 
to  20°/sec  with  G  forces  at  1/2-G.  The  same  basic  inbound  and 
outbound  straight  runs  trajectories  are  used.  Initialization 
errors  ranged  between  20  and  40  feet  in  position  and  -10  and 
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and  35  feet/second  for  velocity.  Figure  34  describes  a  torpedo 
inbound  trajectory  which  maintains  a  Y  distance  from  the  array 
between  -275  and  500  feet.  Four  turns  are  executed  on  this 
run.  The  X  and  Y  position  errors  reveal  essentially  the  same 
characteristics  described  before.  In  general  the  position  and 
depth  errors  decreased  as  the  torpedo  approaches  the  array, 
become  a  minimum  at  the  closest  point  of  approach,  and  increase 
as  the  torpedo  moves  outbound  past  the  array.  The  higher  turn 
rates  experienced  on  this  test  do  not  degrade  the  X  and  Y  posi¬ 
tion  errors.  Figures  35  and  36  show  that  position  and  depth 
errors  ranged  between  -1  and  +5  feet. 

Figure  37  depicts  a  torpedo  path  which  maintains  a  Y  dis¬ 
tance  from  the  array  between  1760  and  2760  feet.  Three  20°/sec 
1/2-G  turns  are  performed  during  this  run.  Figures  38  and  39 
describe  the  position  and  depth  errors  which  range  between  -7 
and  +9  feet  during  steady  state. 

The  diagonally  inbound  trajectory  with  four  20°/sec  1/2-G 
turns  is  displayed  in  Figure  40.  Position  and  depeth  errors 
for  this  run,  Figures  41  and  42,  ranged  between  +10  and  -10 
feet.  Again  note  the  similarity  in  the  position  error  behavior 
for  each  channel  for  this  type  trajectory.  The  larger  errors 
in  position  after  time  slot  125  are  due  to  the  filter's  RMS 
position  error  estimates  increasing  as  the  torpedo  approaches 
the  radial  tracking  limit  of  the  hydrophone  array. 

The  last  set  of  maneuvering  tests  increased  the  G  force 
during  the  20°/sec  turns  from  1/2-G  to  1-G.  The  three  basic 
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inbound-outbound  trajectories  are  again  repeated.  Figures 
43  through  51  describe  the  torpedo  trajectories  and  correspond¬ 
ing  position  and  depth  errors.  Overall  performance  indicates 
that  during  steady  state  conditions  the  magnitude  of  position 
and  depth  errors  are  below  10  feet. 

Due  to  the  presence  of  system  noise  position  errors 
reached  a  maximum  of  14  feet,  when  the  torpedo  trajectory 
exceeded  the  radial  tracking  limit  of  the  hydrophone  array. 

As  was  also  observed  in  the  preceding  tests,  errors  induced 
by  the  turn  rate  appeared  to  have  a  negligible  effect  on  fil¬ 
ter  performance. 

C.  SERIES  THREE— MULTIPLE  ARRAY  TRACKING 

The  purpose  of  the  last  series  of  tests  is  to  functionally 
demonstrate  that  the  filter  is  able  to  track  the  torpedo 
through  multiple  arrays  using  the  simulated  tracking  range 
set  up  and  handoff  scheme  described  in  the  previous  section. 
Figure  52  depicts  a  straight  run  trajectory  which  traverses 
the  six  hydrophone  arrays.  The  position  and  depth  errors 
are  described  in  Figures  53  and  54. 

Note  that  the  handoff  from  array  to  array  was  accomplished 
with  negligible  effect  on  position  and  depth  errors.  The  mag¬ 
nitude  of  the  errors  compare  well  with  those  observed  on  the 
single  array  straight  run  tracking. 
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VII.  CONCLUSIONS 


The  sequential  Extended  Kalman  filter  satisfactorily  pro¬ 
vided  real  time  estimates  of  torpedo  position  and  depth.  The 
magnitude  of  steady  state  position  and  depth  errors  ranged 
between  0  and  10  feet  for  torpedo  tracks  within  the  specified 
radial  tracking  range. 

The  iterative  error  bound  routine  furnished  the  capability 
to  estimate  torpedo  position  and  depth  during  turning  maneu¬ 
vers  up  to  1-G .  Errors  generated  during  maneuvers  are  compa¬ 
rable  to  those  observed  during  straight  runs . 

The  filter's  RMS  estimates  of  position  and  depth  errors 
are  dependent  on  system  noise  and  the  distance  the  torpedo 
is  from  the  center  of  the  hydrophone  array.  In  general  larger 
errors  are  encountered  at  more  remote  distances  from  the 
hydrophone  array. 

The  simulated  trajectories  were  performed  using  constant 
measurement  noise  sequences.  Since  the  filtering  problem  is 
non-linear  the  error,  in  the  filter's  state  estimate  vector  is 
a  function  of  the  statistical  properties  of  the  noise.  Future 
tests  should  include  a  Monte  Carlo  simulation  where  filter 
performance  is  investigated  for  an  ensemble  of  measurement 
noise  sequences. 

Additional  tests  should  also  include  evaluating  filter 
performance  using  trajectories  generated  from  actual  torpedo 
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runs  on  the  Dabob  test  range.  These  tests  would  verify  the 
adequacy  of  the  noise  model  in  the  filter  and  the  ability  of 
the  software  to  edit  erroneous  transit  time  measurements. 
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Figure  4.  True  Trajectory  of  the  Torpedo  in  the  Area 
of  a  Single  Array 


RMS  ESTIMATE  OF  POSITION  ERROR  (FT) 


Figure  5.  Filter  Estimate  of  RMS  Position  Error  During  a 
Straight  Run  in  the  Area  of  a  Single  Array 
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Figure  6.  Error  in  Torpedo  Position  During  A  Straight  Run 
in  the  Area  of  a  Single  Array 
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Figure  7.  Error  in  Torpedo  Deptn  During  a  Straight  Run 
in  the  Area  of  a  Single  Array 
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Figure  8.  True  Trajectory  of  the  Torpedo  During  a  Straight 
Run  in  the  Area  of  a  Single  Array. 
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igure  20.  Error  in  Torpedo  Position  During  a  Maneuvering 
Run  in  the  Area  of  a  Single  Array 
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Figure  27.  Error  in  Torpedo  Depth  During  a  Maneuvering 
Run  in  the  Area  of  a  Single  Array 
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Figure  28.  True  Trajectory  of  the  Torpedo  During  a 

Maneuvering  Run  in  the  Area  of  a  Single  Array 
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Figure  29.  Error  in  Torpedo  Position  Duirng  a  Maneuvering 
Run  in  the  Area  of  a  Single  Array 
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Figure  30.  Error  in  Torpedo  Depth  During  a  Maneuveria 
Run  in  the  Area  of  a  Single  Array 
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Figure  31.  True  Trajectory  of  the  Torpedo  During  a 
Maneuvering  Run  in  the  Area  of  a  Single 
Array 
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Figure  33.  Error  in  Torpedo  Position  During  a  Maneuvering 
Run  in  the  Area  of  a  Single  Array 
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Figure  34.  True  Trajectory  of  the  Torpedo  During  a 

Maneuvering  Run  in  the  Area  of  a  Single  Array 
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Figure  36.  Error  in  Torpedo  Depth  During  a  Maneuvering 
Run  in  the  Area  of  a  Single  Array 
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Figure  37.  True  Trajectory  of  Che  Torpedo  During  a 
Maneuvering  Run  in  the  Area  of  a  Single 
Array 
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Figure  33.  Error  in  Torpedo  Position  During  a  Maneuvering 
Run  in  the  Area  of  a  Single  Array 


ERROR  IN  POSITION  (FT) 


D 00 . 


-2000.  -1000.  0.  1000.  2000.  3000.  400 

X-POSITECN  (FT) 


Figure  A3.  True  Trajectory  of  the  Torpedo  During  a 
Maneuvering  Run  in  the  Area  of  a  Single 
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Figure  46.  True  Trajectory  of  the  Torpedo  During  a 

Maneuvering  Run  in  the  Area  of  A  Single  Array 
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Figure  47.  Error  in  Torpedo  Depth  During  a  Maneuvering 
Run  in  the  Area  of  a  Single  Array 
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Error  in  Torpedo  Position  During  a  Straight  Run  Through 
Multiple  Arrays 
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APPENDIX  A 


PROGRAM  DESCRIPTION  AND  FEATURES 

The  sequential  Extended  Kalman  Filter  routine  used  for 
torpedo  tracking  is  modularized  for  ease  of  implementation. 

The  program  is  general  in  nature  and  many  of  the  parameters 
of  the  filter  are  variable  including: 

a.  The  number  of  states  in  the  filter — N 

b.  The  number  of  random  forcing  functions — M 

c.  The  number  of  measurements — JS 

d.  Number  of  time  slots — JTIME 

The  constant  matrices  PHI,  R,  COVW,  and  GAMMA  are  initialized 
in  the  beginning  of  the  program  using  data  statements.  The 
filter  is  initialized  with  P(l/0)  and  X(l/0)  (initial  covari¬ 
ance  of  estimation  error  and  states)  using  subroutine  INIT. 

The  first  estimate  is  at  time  1  and  continues  until  ITIME  = 
JTIME+1.  True  measurement  times  (Zi)  are  computed  using  either 
subroutines  TRAJEC  or  TRAJC3,  depending  on  whether  single 
array  or  multiple  array  tracking  is  implemented.  Either  sub¬ 
routine  will  compute  four  measurement  times  (Tc,  Tx,  Ty,  Tz) 
for  each  time  slot.  The  measurement  times  are  corrupted  by 
zero-mean,  white  Gaussian  noise  using  the  IBM-360  subroutine 
SNORM.  For  each  of  the  four  time  measurements  the  corresponding 
row  of  the  linearizing  H  matrix  is  calculated  using  either 
subroutine  CHROW  or  CHR0W3,  depending  on  whether  single  array 
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or  multiple  array  tracking  is  used.  The  corresponding  gain 
matrix  column  GI  is  then  found.  These  row  and  column  values 
are  utilized  in  forming  the  covariance  of  estimation  error, 

PI,  for  that  particular  time  measurement.  Next  the  estimate 
of  the  observation  time  ZHAT  from  that  particular  hydrophone 
is  formed  using  the  subroutine  CZHAT  or  CZHAT3,  depending  on 
whether  single  or  multiple  array  tracking  is  implemented.  The 
residual  ZDIFF{I)  =  ZIC{I)  -  ZHAT,  is  then  calcualated.  Finally 
the  estimate  of  the  states  XI  based  on  one  time  measurement  is 
calculated  and  the  process  is  repeated  for  the  next  measure¬ 
ment.  After  the  four  iterations,  four  transit  time  measures 
are  formed  using  the  last  estimate  of  the  states,  X4,  in  either 
subroutine  CZHAT  or  CZHAT 3 .  The  corresponding  four  transit 
time  residuals  are  then  calculated.  The  average  of  the  abso¬ 
lute  value  of  the  time  residuals  is  formed  (ZDIFAV)  and  com¬ 
pared  to  a  preset  threshold  value  in  the  software.  If  ZDIFAV 
is  above  the  threshold,  subroutine  QFIND  is  used  to  calcuate 
a  new  Q  which  is  then  added  to  the  last  covariance  of  error 
estimate,  P4.  The  software  then  reprocesses  the  transit  time 
measurements  for  the  same  time  slot.  This  procedure  continues 
until  the  ZDIFAV  measurement  is  equal  or  below  the  threshold 
value,  at  which  time  the  last  estimate  of  the  state  vector 
and  covariance  matrix  become  the  XKK  and  PKK  values  for  the 
time  slot.  Finally,  the  predictions  XKKMI  and  PKKMI  are  formed 
before  the  process  is  repeated  for  the  next  time  slot.  PTLOP 
is  used  to  generate  line  printer  plots  and  PLOTG  is  used  to 
generate  VERSATEC  plots. 
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A.  PROGRAM  SUBROUTINES 


A  brief  description  of  the  subroutines  that  are  used 
with  the  filter  are  described  below: 

1.  T RAJ EC — This  subroutine  develops  the  torpedo  tra¬ 
jectory  which  is  used  as  truth  data  for  the  filter.  The 
subroutine  generates  constant  velocity  and  heading  tracks, 
turns  of  any  desired  turn  rate  and  G  force  can  be  performed 
at  any  time  along  the  torpedo  track.  The  subroutine  outputs 
true  transit  times,  ZI(I),  and  the  X,  Y,  Z  positions,  TD(I), 
of  the  torpedo  for  each  time  slot.  The  subroutine  is  used 
during  single  array  tracking. 

2.  TRAJC3 — This  subroutine  performs  the  same  function 
as  TRAJEC  but  is  used  only  during  multiple  array  tracking. 

3.  INIT — This  subroutine  generates  the  initial  state 
vector  (X0/-1)  and  initial  covariance  matrix  (P  0/-1) . 

4 .  CHROW — This  subroutine  computes  the  appropriate  row 
of  the  linearizing  H  matrix.  Each  row  corresponds  to  one 

of  the  four  transit  time  measurements,  T  ,  T  ,  T  ,  T  .  This 

L  Y  2 

subroutine  is  used  during  single  state  array  tracking. 

5.  CHR0W3 — THis  subroutine  performs  the  same  functions 
as  CHROW  and  used  during  multiple  array  tracking. 

6.  CZHAT — This  subroutine  computes  the  estimated  transit 
times  for  the  filter.  Four  time  estimates,  ZHAT,  are  calcu¬ 
lated  corresponding  to  each  of  the  four  true  transit  times 
ZI(I).  This  subroutine  is  used  during  single  array  tracking. 

7.  CZHAT3 — subroutine  performs  same  functions  as  CZHAT 
however  it  is  used  only  during  multiple  array  tracking. 
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8.  QFIND — This  subroutine  develops  an  adative  Q  matrix 
which  is  a  function  of  the  torpedo  velocity.  Three  input 
variables  defined  in  a  data  statement  at  the  beginning  of  the 
program  can  be  adjusted: 

aa.  SIGACC — Maximum  expected  horizontal  acceleration 
of  the  torpedo. 

bb.  SIGDIV — Maximum  expected  change  in  vertical 
velocity . 

cc.  SIGCC — Maximum  expected  turn  rate  of  the  torpedo 
in  the  horizontal  plane. 

The  values  listed  in  the  program  were  used  and  kept 
constant  during  the  simulation  tests.  If  the  user  desires 
not  to  use  the  adaptive  Q  subroutine  software  code  is  provided 
at  the  beginning  of  the  program  to  calculate  a  constant  Q 
matrix. 

9.  SNORM — This  is  an  IBM-360  subroutine  contained  in  the 
IMSL  library.  The  routine  generates  zero  mean  white  Gaussian 
noise  with  an  RMS  value  normalized  to  1.  The  main  program 
scales  the  noise  and  adds  it  to  the  transit  times  measurements. 

10.  PLOTP — This  is  an  IBM- 360  subroutine  used  to  generate 
the  line  printer  plots.  Information  on  this  subroutine  can 
be  obtained  from  the  IMSL  library. 

11.  PLOTG--This  is  an  IBM-360  subroutine  used  to  generate 
the  VERSATEC  plots.  Information  on  this  subroutine  can  be 
obtained  from  the  IMSL  library. 
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B.  UTILITY  PROGRAMS 

These  subroutines  were  designed  to  be  used  for  repetitive 
matrix  and  vector  manipulations: 

1.  PROD — multiplying  two  matrices 

2.  MMULT — multiplying  a  matrix  and  a  vector 

3.  VMULT — multiplying  two  vectors 

4.  TRANS — transposing  a  matrix 

5.  ADD — adding  two  matrices 
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APPENDIX  B 

SEQUENTIAL  EXTENDED  KALMAN  FILTER  PROGRAM  LISTING 
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